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Abstract 


We address the problem of navigating a set of moving agents, e.g. auto- 
mated guided vehicles, through a transportation network so as to bring 
each agent to its destination at a specified time. Each pair of agents is re- 
quired to be separated by a minimal distance, generally agent-dependent, 
at all times. The speed range, initial position, required destination, and 
required time of arrival at destination for each agent are assumed pro- 
vided. The movement of each agent is governed by a controlled differ- 
ential equation (state equation). The problem consists in choosing for 
each agent a path and a control strategy so as to meet the constraints 
and reach the destination at the required time. This problem arises in 
various fields of transportation, including Air Traffic Management and 
train coordination, and in robotics. The main contribution of the paper 
is a model that allows to recast this problem as a decoupled collection of 
problems in classical optimal control and is easily generalized to the case 
when inertia cannot be neglected. Some qualitative insight into solution 
behavior is obtained using the Pontryagin Maximum Principle. Sam- 
ple numerical solutions are computed using a numerical optimal control 
solver. 


1 Introduction 

Problems in coordinated motion planning for multiple agents can be 
roughly classified into two disjoint categories, decoupled coordination 
(each agent’s motion is planned separately, then the plans are reconciled), 
and centralized coordination (all the agents’ motions are planned simul- 
taneously, with the interaction constraints considered from the start) [1]. 
The problems considered in this paper fall in the latter category. Cen- 
tralized coordination of multiple agent motion has been approached us- 
ing various types of mathematical models, discrete (see, for example, [2] 
and references therein) and continuous (see, for example, [1,3—5], and 
references therein). A review of research on multi-robot coordination 
problems can be found in [1, section 1.1], 

In a number of coordination problems, the moving agents are confined 
to a transportation network (also known as roadmap coordination space 
[1]). A general mathematical model of a transportation network is a 
multigraph [6] with the vertices being points in a Euclidean space and 
edges being parametrized curves connecting pairs of vertices. Examples 
of such networks include railroad networks for trains, railroad networks 
for industrial robots, trolley and tram car networks, and airspaces with 
fixed nominal routes. 

A subclass of network-confined coordination problems consists of 
those where the agents’ paths are not given but sought as part of solv- 
ing the problem. In such problems, the system exhibits behaviors both 
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continuous (the agent’s motion along an edge) and discrete (an agent’s 
choice between two edges emanating from the same vertex). This cou- 
pling of the two behaviors suggests hybrid control systems (HCS) [7] as 
a suitable class of models for approaching the problem. Hybrid systems 
have been applied to various problems of transportation (e.g., highway 
traffic [8,9]), and in particular to aerospace problems [10-12], and are ap- 
plied here to the problem of finding routes and speed advisories for a set 
of agents moving in a transportation network with separation constraints 
and an arrival schedule imposed. Namely, one is given the following data: 

1. A directed multigraph G = (V,E), each vertex v € V being a 
point in a Euclidean space E of dimension 2 or 3. If e G E is an 
edge from vertex v\ to vertex V 2 , then the nominal route segment 
from waypoint v\ to waypoint t >2 is a curve in E, connecting v\ to 
V 2 - All such curves will henceforth be assumed rectifiable [13] and 
capable of a parameterization which is continuous and piecewise 
continuously differentiable. A cusp in the curve can be traversed 
with the assumption (made throughout this paper, but capable of 
relaxation) that inertia is neglected, and approximately smoothed 
if inertia is to be taken into account. A graph-theoretic path [6] 
in G is, therefore, associated (and, henceforth, identified) with a 
spatial path that can be traversed by a moving agent. A vertex of 
G of indegree > 2 [6] (respectively, outdegree > 2) corresponds to 
two or more route segments merging (respectively, diverging). The 
modeling framework below imposes no restrictions on the outdegree 
or indegree of a vertex. 

2. A finite set 

A={1,,..,A} (1) 

of moving agents a G A in G. If agent a is moving along a path 
in G, the agent’s position is specified by the arc length coordinate 
x a along the path. 

3. For each agent a £ A, a specification of the agent’s initial posi- 
tion x INIT ' ,a , required destination x DEST,a , and the required time 
t DE ST;a Q f arr i v i n g a t the destination. Here x INIT ’ a and x DEST ' a 
are points in G, each point specified, for example, by an edge in G 
and a fractional distance along that edge. 

4. The inertia-free state equations [14] (henceforth the dot denotes 
differentiation with respect to physical time t) 

x a = s a , a G A, 

where the s a ’s are the corresponding speeds, describing the motion 
of those agents a that have not yet reached their destination. In 
what follows, and with the details provided below, the coordinates 
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x a will play the role of state variables ; the speeds s a , of the control 
variables. 

5. State constraints: the separation requirement for each pair of agents. 
This requirement is described mathematically, in terms of the co- 
ordinates x a , in section 3. 

6. Control constraints: bounds on the speeds s" . 

7. A cost functional, specified below. 

The problem, defined in detail below (definition 4.1) as the Scheduled 
Routing Problem, consists in finding for each agent a £ A a path p(a ) in 
G from x INIT ’ a to x DEST ’ a and a control strategy sA a ^(t) (i.e. , a function 
from the time domain of the model to the set of admissible controls) 
such that the resulting movements x a (t ) along the corresponding paths 
constitute a state trajectory that satisfies the above state and control 
constraints and that minimizes the cost. 

In this paper, we use an HCS framework to formulate a model spe- 
cialized to the above problem. The main contributions of this model are 
as follows: 

• Reduction of the problem to a special case of an HCS where each 
solution trajectory lies in only one control mode. 

• A clear application of Depth-First Search [15] to search through 
the control modes as economically as possible given the possibly 
exponential size of the problem. (In the worst case when every 
agent can be assigned to any of the paths, and when each edge can 
serve as a path by itself, the number of agent-to-path assignments, 
i.e. of functions /a : A — > E, is |£'|I" 4 L This upper bound, however, 
is a crude overestimate. Better ones are discussed below.) 

• Reduction of each control mode to a problem in classical determin- 
istic optimal control, which allows, at least in principle, application 
of the fundamental results of Pontryagin [16] and Bellman [17], and 
of the numerical algorithms that have been developed and imple- 
mented [18-20]. 

• A natural way to capture an agent’s exiting the system; see Remark 
4.1, below. 

These contributions together allow for parallel computation of solutions: 
the classical optimal control problems corresponding to different control 
modes can be solved in parallel, and their obtained minimal costs values 
compared. Furthermore, the Depth-First Search algorithm itself admits 
a parallel implementation [21]. 

The new hybrid model is formulated in section 2. The classical de- 
terministic optimal control corresponding to a given control mode is 
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formulated in section 4. Numerical solutions to some instances of the 
problem are given in section 5. 

2 An HCS formulation 

The HCS defined in this section will be instrumental in a precise formu- 
lation of the Scheduled Routing Problem. Assume the data l)-7), listed 
in the third paragraph of section 1. 

• For each agent a £ A, let e INIT ’ a denote the edge occupied initially 
by agent a. (Two or more agents can occupy the same edge.) 

• Let V (e INIT ' a , e DEST ’ a 'j be the set of all paths in the multigraph 
that begin with the edge e INIT,a and end with the edge e DEST ’ a 
that contains x DEST,a . The length of a path p £ V (e INIT,a , e DEST ' ,a 
will be denoted l(p). 

• Define a control mode p as a mapping that assigns each agent a to a 

path in V [e INIT ' ,a : e DEST \° ^ more detail, p is a mapping from 

the set A of moving agents to the union U a V ^ e INIT ’ a ^ e DE 8T-,a' s j 
such that 


p(a) £ V [e INIT,ot , e DEST,a ^ for each a £ A 


• For agent a, each path p(a) £ V ^ e INIT '’ a ^ e DEST '’ a 'j i s parameter- 
ized by arc length. For computational convenience, the arc length 
coordinate increases along the path, with the destination coordi- 
nate x DEST ’ a being zero for each a. Thus, £ [— l(p(a)), 0], 
and 

x DEST;a = q 

This convention ensures that an agent’s destination is a vertex in 
the transportation network and, furthermore, that it is the same 
vertex in all control modes. 

• Each agent a in each control mode p is required to reach its des- 
tination = x DEST ' ,a = 0 at a prescribed time t DEST,a . Upon 
reaching destination, the agent is no longer in the model; this is 
reflected in the restriction on the time domains of the individual 
equations in the dynamical law (2), stated below. 

• In each p, have the arc length coordinate evolve according to 
the dynamical law 

Vt(t) = s«(t) for 0 < t < t DEST ’ a , a £ A, (2) 
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where s“ is the control variable corresponding to the agent’s speed 
of motion along the path. In this formulation, the equations that 
constitute the dynamical law are imposed over different (albeit 
overlapping) time domains. The problem, however, will be con- 
verted below to one where all dynamical law equations are imposed 
over the same (rescaled) time domain. 

• For each /r and each a , impose the arrival requirement 

x“ ( t DEST 

• For each pc and each a , impose the speed ranges 

MIN;a a ^ MAX-, a 

^ 

3 The geometry of separation constraints 

In some transportation types, including aircraft and trains, every pair of 
moving agents must be be separated by a distance no smaller than a pre- 
determined minimal separation. For example, the minimal separation 
requirements for air traffic in the U.S. are defined in [22] and depend on 
numerous factors, including airspace type, air traffic automation systems 
in use, and aircraft weight classes (the classes defined in [22] are: Small, 
Large, Heavy, B757). Other types of moving agents, e.g. industrial 
robots, may also be subject to complicated, asymmetric, and anisotropic 
(e.g., for aircraft, altitude-dependent) separation requirements. The sep- 
aration requirement will be a key constraint on the state variables in the 
Scheduled Routing Problem formulated in section 4.2. 


= x DEST;a 


( 3 ) 



(a) (b) 

Figure 1. Agents 1,2 on their respective rectilinear edges ei,e 2 , which 
share a common vertex, taken as the origin 0 in R 2 . The orientation 
of the edges is not specified, (a) The unit vectors ai,a 2 are collinear 
with the respective edges, but their directions do not necessary agree 
with the edges’ orientations, (b) With suitably chosen scalar coefficients 
ci, C 2 , the vectors ciai and C 2 a 2 are the respective position vectors of the 
two agents. 
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No attempt is made in this paper to capture all such requirements in 
detail (see, however, section 6.3) for a discussion of anisotropy in sepa- 
ration requirements arising from altitude dependence). Instead, we will 
use conservative approximations, addressing only the following asym- 
metry: if two moving agents are in-trail (i.e. , one is directly following 
the other along a route segment which is not necessarily in a horizontal 
plane), then the minimal separation can depend on the weight class of 
the leading and trailing agent. To capture this potential asymmetry, 
for each pair a 1 , 0:2 of agents with the first one leading, we introduce 
the minimal separation r ai a2 . If the asymmetry takes place, it can be 
written 

T'ai,a2 7 “ ^ot.2-, cxi ( 5 ) 

We now calculate the set of all states, in a control mode /i of a hybrid 
system described above, where at least two agents violate the separation 
requirement. The scenario shown in Figure 1A has two agents on two 
different rectilinear edges, which need not lie in a horizontal plane, with a 
common vertex and no specified orientation. (If the edges are curvilinear 
with low curvature near a common vertex or intersection, these portions 
can be approximated by linear segments; otherwise, the analysis becomes 
considerably more complicated.) 

Remark 3.1. Since edge orientation is not specified, Figure 1 describes 
four cases: one where both agents are moving toward the common vertex, 
one where both agents are moving away from the common vertex, and 
two more cases in which one agent is moving toward, and the other away 
from, the common vertex. 



Figure 2. An example of two elliptical sectors in the ciC 2 -plane corre- 
sponding to conflicting states. 

The asymmetry of the gray-shaded region about the dashed diagonal 
is the asymmetry (5). 

We will use the Euclidean inner product (-, •) and the corresponding 
norm || ■ || in the 2-D space containing the two edges. Pick the common 
vertex as the origin and the unit vectors ai, a 2 as the basis vectors that, 
regardless of the edge orientations , point from the origin toward the 
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respective agents. With suitable scalars ci,C 2 , the vectors ci&i and C 2 a 2 
are the agents’ respective position vectors. The squared distance between 
the two agents is 

||ciai - c 2 a 2 || 2 = (ci) 2 + (c 2 ) 2 - 2cic 2 (ai,a 2 ) (6) 

Equating the latter expression to the squared minimal separation, say, 
r 2 2 , we obtain the equation 

(ci) 2 + (c 2 ) 2 - 2cic 2 (ai,a 2 ) = r 2 2 (7) 

of an ellipse in the ciC 2 -plane. The corresponding set of conflicting sets 
is described by the elliptical sector obtained by intersecting the ellipse- 
bound region 

(ci) 2 + (c 2 ) 2 - 2cic 2 (ai,a 2 ) < r 2 2 

with the open octant c 2 > c\ > 0, corresponding to the case when agent 
1 is the one closer to the origin. In the other case (agent 2 is closer to 
the origin) , the corresponding elliptical sector is obtained by intersecting 

(ci) 2 + (c 2 ) 2 - 2cic 2 (ai, a 2 ) < 

with the octant c\ > c 2 > 0. The role of the angle 6 between the edges 
ei,e 2 in both sectors is the equality (ai,a 2 ) = cos(0). An example of 
two such sectors is shown in Figure 2. 



Cl 


Figure 3. An example of two stripes in the ciC 2 -plane corresponding to 
conflicting states of two agents on the same edge. 

In each of the four cases listed in Remark 3.1, the respective continu- 
ous state coordinates xj v xj L of agents 1, 2 in control mode [i map to the 
coefficients ci,C 2 , as follows: 

1. If both agents are moving toward the common vertex, then x" = 
l(e a ) — c a for a = 1, 2. 

2. If both agents are moving away from the common vertex, then 
x" = c Q for a = 1, 2. 
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3. If agent 1 is approaching, and agent 2 going away from, the common 
vertex, then = l(e i) — ci,x^ = C 2 - 


4. If agent 2 is approaching, and agent 1 going away from, the common 
vertex, then x * = c \ , xj) = l ( 02 ) — C 2 - 


If 6 > 90°, then in the last two cases /U allows only one in-trail sequence, 
so the minimal separations used for the two sectors in Figure 2 are equal. 
If the two agents 1, 2 are on one and the same edge, then the set in the 
ciC 2 -plane of the conflicting states appears as in Figure 3 (the asymmetry 
about the dashed diagonal corresponds to (5). The mapping from the 
continuous state coordinates x*, x j) to the coefficients ci, C 2 is constructed 
analogously to the above four cases. 




Figure 4. An example of two agents whose paths in the transportation 
network are prescribed and overlap. The black star shows the beginning 
of the overlap in (a) and the corresponding state (both agents being 
at that point) in (b); the white star, the end of the overlap in (a) and 
the corresponding state (both agents being at that point) in (b). The 
system, shown in (a), has seven control modes with both agents in the 
transportation network. Each mode’s set of separation-violating states, 
shown in (b) as a connected [23] gray region, is “glued” to some of the 
others. The result of the gluing is the connected region shown in (c). 


The above calculation is illustrated, for an example of two moving 
agents, in Figure 4. In each control mode, the set of conflicting states is 
shown as a connected [23] gray region. For dimension A above 2, one must 
compute for each pair of agents the set of states violating the separation 
requirements. Each such set is a cylinder, or union of cylinders, with the 
base shaped as shown in Figure 4c, in the total state space We 

note that the set of all separation-violating states in is cylindrical 

in the sense of [5, Definition 2.2], the latter definition a key requirement 
for the applicability of a number of theoretical results of [5] . 


4 The Scheduled Routing Problem and the equiv- 
alent Stacked Scheduled Routing Problem 


For ease of exposition, we precede the general formulation of the problem, 
suitable for an arbitrary number of moving agents, with a specific, two- 
agent, example. 


4.1 An instance of the scheduled routing problem for two 
agents 




(a) (b) 

Figure 5. An initial state (a) and required destinations (b) of a two-agent 
set. 

The initial state of a two- agent scheduled routing is shown in Figure 5a; 
the required destinations (with possibly different required arrival times) 
for the two agents, in Figure 5b. In this Figure, 

a = 1 : e mT;1 = e 1; e DEST - 1 = e 8 , 
a = 2 : e INIT ;2 = e 2 , e DEST ;2 = e 7 

The paths are 

Pi ■ ei, e3, e6, e 8 ; P2:ei,e4,e 8 ; ps : e 2 , e3, e^, P4:e2,e4,e7 (8) 

Agent 1 can take either p\ or p 2 ] agent 2, either p% or p^. Consequently, 

V(e’«'T\ e OE S T-„) = {pip2)i v ^mT» e DEST^ = 

No other paths can be taken. 
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Figure 6. The four paths (8), in that order, for the problem in Figure 5. 


The paths pi, ■ ■ ■ ,P 4 are shown, in that order, in Figure 6. We obtain 
the control modes p i,...,p 4 , defined as follows: 


a 

Pi (a) 

p 2 (a) 

p 3 (a) 

P4(a) 

1 

Pi 

Pi 

P2 

P2 

2 

P3 

Pi 

P3 

Pi 


The state space corresponding to each control mode p is a rectangle 
consisting of those state vectors ( x*,x ^ that are compliant with the 
arc length bounds and separation constraints. 

The above system is subject to the operational requirement (3), here 
for a £ A = {1, 2}, that agents 1 and 2 arrive at their destinations at 
times t DEST ' 1 ,t DEST ’ 2 , respectively. 
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(a) (b) 

Figure 7. Topology of approximated conflict zone in the state spaces (a) 
X /n . X IM and (b) X /l2 , X /l3 . The black dot (near top right) denotes the 
required destination coordinate pair (x EEST ' 1 , x EEST,2 ^j . 

In each of // 1 , // 4 , the pairwise conflict zone is simply connected [23] 
(i.e., consists of only one connected component and has no “holes”). 
Consequently, once each of the conflict zones, of the form shown in Figure 
4c, is approximated by an ellipse-bounded region, the state spaces for 
Hi, Hi have the topology shown in Figure 7a. In each of the two 

agents’ paths have two crossings (regarded here as short overlaps), hence 
the pairwise conflict zone has two connected components (Figure 7b). 
Each control mode /i is subject to the initial condition 

x“(0) = x INIT ’ a for a € A (9) 

The simplifying assumption underlying (9) is that both agents start their 
journey simultaneously. This assumption can be relaxed and is used here 
for mathematical simplicity only. 

Finally, for each control mode /i, we can specify a cost functional 
that suits the goals implied by the context of the specific application. 
One example is a cost functional that requires the agents (e.g., trains) 
to move as slowly as possible, for safety, is 

f t DEST ’<* 

£ L (»;) * ( m » 

aeA J0 

Thus, for each control mode /i, we have an optimal control problem 
with dynamical law (2), subject to the initial condition (9), the control 
constraints that specify permissible value ranges for the speeds s“, and 
the following additional constraints: 

• The arrival requirement (3). 

• The separation requirement , defined by constructing a function 
9 EEP ’ ai,a2 {{ x< jl)aeA) °f the state such that the pairwise conflict 
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zone (shown as a gray-shaded region in the appropriate panel of 
Figure 7) is exactly the set of states satisfying the functions 


SEP,a 1: a2 


> r 


2 

OL\ ,Q!2 ’ 


( 11 ) 


where each g SEP '’ a u a 2 (x ai , x° 2 ) is a quadratic function of the form 
given by the left-hand side of (7). 


For the pair (ctq = 1 , aq = 2) (and, in the general scheduled routing 
problem, for every pair (aq, 012 )) of agents, inequality (11) defines a union 
(denoted X® 1,a2 ) of regions in the state space of //, each region bounded 
by an ellipse-cylindrical (“tube-shaped”) hypersurface. The separation 
requirement thus translates into the requirement that a solution (the 
state trajectory) is disjoint from the interior of every such region for 
every pair of agents. 


4.2 The Scheduled Routing Problem: a general formula- 
tion 

The central problem of this paper, which will be called the Scheduled 
Routing Problem , can now be stated as follows. 

Definition 4.1. (Scheduled Routing Problem) Given a set A of agents 
moving on a route network G = (V, E) subject, in each control mode p, 
to the dynamical law (2), the initial condition (9), and the control con- 
straints (4), find a control strategy (s^(t))q, such that the corresponding 
state trajectory x^(f) = (x“(t)) Qe ^ that satisfies (3) and minimizes the 
total cost. 

This is an optimal control problem with a Lagrange cost function. 
The problem, however, has two non-standard features that hamper ap- 
plication of classical optimal control theory and numerical computation 
of solutions. One feature is the presence of intermediate constraints. 
The other is the following non-autonomous behavior: an agent, once at 
destination, no longer “participates” in the constraints or in the cost. 
We now use a formalism similar to that in [24] to reduce this problem 
to a classical, optimal control problem, which is autonomous if its cost 
functional is. In the rest of this section, the subscript /i is dropped for 
brevity. 


4.3 The Stacked Scheduled Routing Problem (SSRP), equiv- 
alent to the Scheduled Routing Problem 

Let {a q )q =1 be an ordering of the agents by their arrival times (arranged 
in nondecreasing order). Let to = 0, and for each q > 0 let 

^ j.DEST;a q 

Define a new (normalized) time r e [0, 1] and, for each of the intervals 
[t q , t q+ 1 ], q = 0, . . . , A — 1, introduce the following: 
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the state variable p q (r), which will play the role of “physical time’ 
in the interval [t q ,t q + 1 ] : 


tq Pq{ T) < tg+lj 

• the state variables y“(r), a G A, related to the above x a (t ) by 

y q (r) = x a (t ) if p q (r) = t; (12) 

• the vector notation y q (r) = {y q (T)) a eA\ 

• the control variables s“(t), related to the above s a (t) by 

s q {r) = s a (t) if p q {r) = t, (13) 

• the control variables z q > 0, which represent the rate of flow of 
physical time with respect to the normalized time r. 

The above definition of the Lagrange cost reflects the assumption that 
an agent, once at destination, “disappears” from the system, in the sense 
of being no longer subject to the separation requirement with the other 
agents. From (2), (12), and (13), one readily obtains the dynamical law 
equations 


^yq q ' = ZqSq q ’ for (/ >q + l } 

> a € A 

c b p< i = Zc t \ 


(14) 


Finally, the initial conditions and arrival requirement, together with the 
requirement that physical time and state change continuously when pass- 
ing from one interval [t q ,tq+ 1 ] to the next, translate to the endpoint 
constraints 


Vo(0) 

_ x INIT;a ^ gee (g)) 

(a) 

Pq( 1) 

= Pq+ 1(0) 

(b) 

A7+l(0) 

= tq+1 

(c) 

v7\ !) 

= Vg+x (o) for <:{ >q + l 

(d) 

y q ?i\ i) 

__ x DEST;a q+ 1 

(e) 


Conditions (15. be) ensure the continuity of physical time flow; conditions 
(15. d), the continuity of an agent’s motion. 
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Remark 4.1. The state equations (14) and the endpoint constraints 
(15.d) come with the restriction q' > q + 1 because they are imposed, 
in agreement with (2), only for those agents who have not yet reached 
their destination: by the end of the q-th time period, the first q agents 
have reached destination. Thus, upon reaching destination, an agent is 
excluded from the model, and is no longer represented by a state equation 
or subject to separation constraints with the other agents. This eliminates 
the necessity to continue modeling an agent whose role in the model has 
already been fulfilled. 

Throughout the rest of this paper, the choice (10) of the cost func- 
tional is assumed in all the numerical examples. Other choices of the cost 
functional are discussed in section 6. Since the time intervals between 
each consecutive pair of arrivals are modeled, in this latter formulation, 
as if they were occurring simultaneously (“stacked” upon one another), 
the following definition will be adopted. 

Definition 4.2. (a) The newly obtained optimal control problem corre- 
sponding to a control mode p and consisting of the state equations (If), 
endpoint constraints (15), cost functional (10), the separation constraints 

OL / 

on the variables y q q , the control constraints corresponding to (4) sped- 

Ol. / 

fying the speed ranges on the variables s q q , and the positivity constraints 

Zqd 0 , 

will be called a /i-stacked optimal control problem. 

(b) The set of all p- stacked optimal control problems will be called a 
Stacked Scheduled Routing Problem (SSRP). 

(c) Of all the optimal solutions to all the p-stacked optimal control 
problems, a solution achieving a lowest cost is called an optimal solution 
to the SSRP. 

Thus, an SSRP consists of a collection of optimal control problems, 
and an optimal solution to the SSRP tells not only how quickly the 
agents are to move, but also how they should be routed. The SSRP is 
equivalent to the Scheduled Routing Problem (definition 4.1). 

4.4 Implications of the Pontryagin Maximum Principle 
for the SSRP 

The assumption that we are in a specific control mode p is still in force. 
Denote by £,y q ,f Pq the costates for y q ,p q . In those states where none of 
the state constraints is active, the Hamiltonian for each control mode of 
the SSRP is 


H — fo + 51 z q s q q (,y q + 5Z Z| j£/V 

q;q'>q+l 1 
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where /o is the performance index (running cost) corresponding to the 
cost functional (10). Since H does not explicitly depend on any of the 
state variables, it follows that the costate variables are constant along 
the trajectory portions clear of state constraints and, consequently, the 
maximization of H in each such state is a problem of static maximization. 
This fact, in turn, implies the existence of an optimal state trajectory 
in which these portions are rectilinear on each [t q ,t q + 1 ]. If, furthermore, 
there is only one optimal trajectory, then its portions away from the 
obstacle boundary are necessarily rectilinear. 

With the state equations (2) and the set (4) of control constraints, the 
set of all states reachable from a given state y° = {y^ a ) a &A is the pointed 
polyhedral cone that consists of all states y^ = (y^) a eA satisfying 



/ 

\ 

MIN: oc-\ 

< 

w 

-y°/i ai ) 

MAX ;ao 

< 

y £ 2 - 

„.0;a2 

\ 

MAX-cl-x 

< 

fe 1 


M I N ;ck2 


a\ / 02 


This cone has vertex y° and is the intersection of the half-spaces 


1( « 2 _ „.0;a2 

y fi y fi — 

v7 - vT 2 > 


„MAX;a i 



MIN;ct2 

Sfi 

MIN-, a! 



MAX ;a 2 
S/2 


(<‘ -!#“■)■ 


a l / «2, 


«i f a 2 , 


and 

a € A 

The narrower the speed ranges (4), the narrower the cone, and the 
smaller the portions of an optimal trajectory that lie on the boundary 
of the obstacle 


I | \ r ai,&2 

u ai^a2 yX /i 

This suggests that, for narrow speed ranges, optimal trajectories for 
the Stacked Scheduled Routing Problem can be well approximated by 
piecewise linear curves. 


4.5 An approach to computing solutions and an analysis 
of the associated costs 

The number of the control modes y is the product 

[] | ( e JiV7T ; v DEST;a ) 

a£A 

Paths which are, or contain, cycles are allowed in the model and can be 
desirable in some applications; e.g., in air traffic models where aircraft 
may be sent into a holding pattern to absorb delay. Furthermore, the 
following holds true: 
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Remark 4.2. Every solution to the control problem, described above will 
correspond to exactly one of the control modes p. This property of the 
problem removes two difficulties associated inherently with hybrid systems 
and absent from classical control systems, the risk of excessively frequent 
switchings of control mode, and the necessity for “ control mode memory, ” 
i.e. for keeping track of the control modes entered prior to the current 
time in the system's evolution. 

Each set V [e INIT ' a , e DEST '^ can be computed using the Depth- 
First Search algorithm, whose computational cost in a non-parallel im- 
plementation is known [15, section 23.3] to be 0(|V| + \E\). 

These considerations suggest the following procedure for finding an 
optimal solution to an SSRP. Upper bounds on the computational cost 
are provided, in square brackets ([]), where possible. 

• Compute all the sets V [e INIT ' ,a , e DEST ’ a ^ . [In a non-parallel im- 
plementation, this computation amounts to a Depth-First Search 
for each agent a € A, hence the computational cost is 0(|A|(|U| + 

• For each p, compute an optimal solution to the corresponding 
Stacked Scheduled Routing Problem, and the cost of that solu- 
tion. [The computational cost depends on the particular choice of 
the computational method; see, for example, [18,25]. The numeri- 
cal results in this paper were produced using Sequential Quadratic 
Programming (SQP) [18], chosen for the convenience of available 
software.] 

• Select a control mode p* such that C' /t * < for all p, and declare 
the corresponding optimal solution the optimal solution for the 
SSRP (definition 4.2). [0(the number of control modes).] 

5 Sample numerical computations for the Stacked 
Scheduled Routing Problem 

5.1 Assumptions and notational conventions 

All the transportation networks appearing in the numerical examples 
of this section are graphs; namely, no two vertices are connected by 
more than one edge. This allows to specify each path as a sequence of 
vertices, rather than of edges. To simplify the computations, the sepa- 
ration requirements for each pair of agents are assumed symmetric. The 
numerical code admits a straightforward, albeit somewhat cumbersome 
algebraically, generalization that will dispense with this assumption. 

In panels (a, b) of Figures 9, 11, 12-17, the computed trajectories and 
controls are plotted using the symbols described in Table 1. In panels 
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Quantities Plot symbol 


v\>a\ 

Vq,sl + 

y'q-4 


} for q = 


Table 1. The legend used in Figures 9, 11, 12-16, below. 


(c) (and, when present, (d)) of these Figures, as well as in Figures 8 and 
10, the transportation networks are depicted as directed graphs; moving 
agents, as points labeled with values of a, each point serving as a center 
of a circle with radius equal to half the required pairwise separation. In 
all plots, axis label p refers, in agreement with the above, to physical 
time. All plots were generated using the MATLAB software [26]. 

The first two of the examples in section 5.4 are “abstract,” in the 
sense that no particular application is specified for them. Thus, the units 
of length and physical time are left unspecified. Application and units 
are, however, specified for the third example. The required destination 
yDEST;a - g j n eac h case enc i Q f the p a ^h assigned to agent a in control 
mode //: 


y DEST;a 


= 0 


5.2 The cost function 

The cost function used in the following examples is (10), which, on con- 
verting the scheduled routing problem to a Stacked Scheduled Routing 
Problem (definition 4.2), takes the form 

£ £ 

q=0q'>q+l J{J 


5.3 Computational methods 

In each control mode, the corresponding optimal control problem was 
solved using the OCP solver [20], which uses Sequential Quadratic Pro- 
gramming (SQP) [18]. 
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5.4 Numerical examples 

5.4.1 Three moving agents, one control mode 



-2 -1012 


Figure 8. The transportation network and the agents’ initial locations 
in the example of section 5.4.1. 


The transportation network for this example is the 2-dimensional di- 
rected graph shown, together with the three agents’ initial locations, in 
Figure 8. Each agent can traverse only the edge on which it is positioned 
initially, hence only one control mode fi arises. 

The speed ranges are given by 


a 12 3 

s MiN-, a 0 _3 0 _3 a4 

s M AX -,a 15 o.8 0.9 

h 1 

The required times of arrival at destination are 

a 12 3 

t UEiiT - a 2 0 3 0 4 0 

The minimal required separation is 0.3. 

The numerical solution computed for the control mode /i is shown in 
Figure 9. 
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(a) (b) 




Figure 9. Numerical solutions (for the only possible control mode) in the example of section 5.4.1. (a) State trajectory vs. time, 
(b) Control strategy vs. time, (c) The positions of the agents in the transportation network at p q ( 1) for q = 1. (d) The positions 
of the agents in the transportation network at p q ( 1 ) for q = 2. 


5.4.2 Two moving agents, four control modes, wide speed 
ranges 


Initial state 



Figure 10. The transportation network and the agents’ initial locations 
in the example of section 5.4.2. 

The transportation network for this example is the 2-dimensional di- 
rected graph shown, together with the two agents’ initial locations, in 
Figure 10. 

The two paths considered here are 


pi : vi,V2,v 8 ,vg,vio,ve; p 2 : vi,V2,v 8 ,v^,v 5 ,vq 
The speed ranges are given by 

a 1 2 

0.6 0.6 

s MAX-,a 14 14 

h 1 

The required times of arrival at destination are 

a 1 2 

t UKiiT ' a 2 8.3 36 1 

The minimal required separation is 1.0. 

The control modes are as follows. 


a. 

m(a) 

p 2 (a) 

M3 («) 

pi(a) 

1 

Pi 

P2 

P2 

Pi 

2 

P2 

Pi 

P2 

Pi 


20 


the agents’ arc length coordinates 



(a) (b) 



(c) 


Figure 11. Numerical solutions for control mode in the example of section 5.4.2. (a) State trajectory vs. time, (b) Control 
strategy vs. time, (c) The positions of the agents in the transportation network at p q ( 1) for q = 1. 


the agents’ arc length coordinates 



(a) (b) 


End ot time interval #1 : p = 28.3 



(c) 


CN 

CN 


Figure 12. Numerical solutions for control mode fi 2 in the example of section 5.4.2. (a) State trajectory vs. time, (b) Control 
strategy vs. time, (c) The positions of the agents in the transportation network at p q ( 1) for q = 1. 


the agents' arc length coordinates 



(a) (b) 


End of time Interval #1 : p = 28.3 



(c) 


Figure 13. Numerical solutions for control mode ^3 in the example of section 5.4.2. (a) State trajectory vs. time, (b) Control 
strategy vs. time, (c) The positions of the agents in the transportation network at p q ( 1) for q = 1. 


the agents’ arc length coordinates 



C/D 0.76 - 

“O 
CD 

0 0.74- 

Q_ 

C/D 

- 0.72 - 

C/D 


c 

CD 
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0.7 - 


(a) 
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End of time interval #1 : p = 28.3 



(c) 


CN 


Figure 14. Numerical solutions for control mode /r 4 in the example of section 5.4.2. (a) State trajectory vs. time, (b) Control 
strategy vs. time, (c) The positions of the agents in the transportation network at p q { 1) for q = 1. 


The respective numerical solutions for these modes are shown in Fig- 
ures 11-14. 

The total costs for each control mode are as follows: 


control mode 

Mi 

M2 

M3 

M4 

total cost 

37.5823 

38.8925 

45.4652 

31.5979 


Therefore, the optimal routing and control strategy are achieved in con- 
trol mode ji 4 . 


5.4.3 Two moving agents, four control modes, narrow speed 
ranges 

In this example, the transportation network, 3-dimensional, models a 
terminal airspace, which consists of two arrival paths merging into the 
same final approach segment. The transportation network and the initial 
positions of the two aircraft are shown in Figure 15. 

Here the edge {vq,vj) is the final approach to a runway, and the 
moving agents are aircraft. For realism, one unit of arc length is taken 
here to be 3 nautical miles, and the unit of speed is taken to be 200 
knots, a typical speed allowed in U.S. terminal airspaces at altitudes 
below 10, 000 feet. 

The two paths considered here are 


Pi : v 1 ,v 2 ,v 8 ,vg,vio,v 6 -, p 2 : Vi, v 2 , v 3 , v 4 , v 5 , v 6 
The control modes are as follows. 


a 

Mi («) 

M2 (a) 

M3 (a) 

M4 (a) 

1 

Pi 

P-2 

P2 

Pi 

2 

P2 

Pi 

P2 

Pi 


The speed ranges are given by 

a 1 2 

s“ JJV;a 0.8 (= 160 kts) 0.8 
s MAX;a 1 2 (= 240 kts) 1.2 

The required times of arrival at destination are 

a 1 2 

t ubib-r-,a 21.2 (= 0.03 hrs) 28.1 (= 0.04 hrs) 

The minimal required separation is 1.0 (= 3 nmi). 
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Figure 15. The 3-dimensional route network and the moving agents’ initial positions (centers of the circles in horizontal planes), 
for the numerical example of section 5.4.3. Here the route network is a terminal airspace, and the agents are arriving aircraft. The 
radius of each circle is half the minimal required pairwise separation of 3 nmi. 



1 .05 r 



Figure 16. Numerical solutions for control mode in the example of 
section 5.4.3. (a) State trajectory vs. time, (b) Control strategy vs. 
time, (c) The positions of the agents in the transportation network at 
p q (l) for q = 1. 
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(c) 

Figure 17. Numerical solutions for control mode ^4 in the example of 
section 5.4.3. (a) State trajectory vs. time, (b) Control strategy vs. 
time, (c) The positions of the agents in the transportation network at 
p q (l) for q = 1 . 
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In control modes /I 2 , f-i ;$ , the control problem has no solution, since 
the path lengths are such that the imposed speed ranges prevent at least 
one agent from reaching its destination at time. The numerical solution 
for control mode // 1 is shown in Figure 16 and incurs a total cost of 
49.2396. In 114 , the computed optimal control strategies prescribe the 
constant speeds 

si, =1.03, si, =0.82 

and the solution (Figure 17) incurs a total cost of 41.5594. Therefore, 
the optimal routing and control strategy are achieved in control mode 
A4- 


6 Discussion 

The above modeling framework addresses the problem of navigating a 
set of moving agents in a transportation network, with constraints on 
the agents’ initial locations, required destinations, and required times 
of arrival at destination, as well as on the agents’ minimal pairwise dis- 
tances. We now discuss several directions in which the above model can 
be varied and generalized. 


6.1 A model that includes inertia 


Inertia can be included by treating both the y“’s and the s“’ s as state 
variables, and the accelerations a“ as the control variables. The corre- 
sponding new state equations would assume the form 


Vl = «£ \ 


a <G A 


The rescaling of physical time to normalized time r € [0,1] formulated in 
section 4.2 would be carried out analogously. The resulting problem falls 
in the class of the kinodynamic motion planning problems; some related 
theoretical results can be found in [5] and in references therein. 


6.2 Different choices of the cost function 

Of the vast number of possible choices of cost function, we briefly discuss 
two. 


• In situations where inertia cannot be neglected and acceleration 
must be the control variable (or among the control variables), it 
may be desirable to keep the movement as smooth as possible, 
e.g., for passenger comfort or cargo safety. A cost function that 
would serve this goal is the integral of the sum of the squared 
accelerations: 




dt 
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• In situations where it is undesirable to impose times t DEST;a of ar- 
rival at destination as rigid constraints, and preferable to minimize 
the absolute differences between the actual arrival times t AAT ' ,a . 
This goal would be served by taking the sum 


y ^ DEST-a 
ae A 


- t AAT ’ a ) 


2 


as the cost. 

6.3 Asymmetric and anisotropic pairwise separation re- 
quirements 

In some applications, the pairwise separation requirements for the mov- 
ing agents can be asymmetric or anisotropic, or both. Such is the case 
when the moving agents in question are aircraft traveling along their 
nominal routes. An example of asymmetry is as follows. If two aircraft 
are consecutively in-trail and at the same altitude, and the leader’s and 
follower’s respective weight classes [22] are Heavy and Small, the required 
separation is considerably larger than if the two engine types were in the 
opposite order. An example of anisotropy is the requirement of vertical 
separation between two aircraft, where aircraft are required to maintain 
either 1000 ft vertical separation or the prescribed lateral separation pre- 
viously discussed; the resultant shape of an aircraft’s safety envelope is a 
cylinder. A mathematical form for such an anisotropic constraint would 
use, not the Euclidean norm, but one of the following form: putting 
a k = (of , cl\i af.), the norm in the left-hand side of (6) would be replaced 
by a “mixed” norm, Euclidean in the xy-plane, and the max-norm along 
the height z. If r were the minimal horizontal separation, imposed when 
the two aircrafts’ altitudes differ by less than a quantity h, the mixed 
norm and the corresponding separation constraint would be 


ci 3-i C 2 & 2 1 [mixed 


:= max 



> 1 

The computations in section 3 would have to be modified accordingly 
and would no longer have the closed quadratic form. 
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